package com.gitee.wsl.mathematics.matrix.mat4.ext

import com.gitee.wsl.math.radiansToDegrees
import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.geometry.EulerOrder
import com.gitee.wsl.mathematics.matrix.ext.timesAssign
import com.gitee.wsl.mathematics.matrix.mat3.Matrix3
import com.gitee.wsl.mathematics.matrix.mat4.Matrix4
import com.gitee.wsl.mathematics.matrix.mat4.MutableMatrix4
import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.mathematics.vector.vec3.MutableVector3
import com.gitee.wsl.mathematics.vector.vec3.Vector3
import com.gitee.wsl.unit.angle.AngleUnit
import com.gitee.wsl.unit.angle.ext.cos
import com.gitee.wsl.unit.angle.ext.cosF
import com.gitee.wsl.unit.angle.ext.sin
import com.gitee.wsl.unit.angle.ext.sinF
import com.gitee.wsl.unit.angle.ext.times
import com.gitee.wsl.unit.angle.ext.unaryMinus
import kotlin.math.asin

fun<T:Number> MutableMatrix4<T, *,*>.rotate(ax: T, ay: T, az: T, angle: AngleUnit){
    val temp = copyEmpty()
    temp.setToRotation(ax, ay, az, angle)
    this *= temp
    /*return up {
        tmpMatA.setToRotation(ax, ay, az, angle)
        set(mul(tmpMatA, tmpMatB))
    }*/
}

/**
 * rotate [degreesInAngles] around [Vector3.UNIT_Z]
 */
fun<T:Number> MutableMatrix4<T, *, *>.rotate(degreesInAngles: AngleUnit) = rotate(zero,zero,one, degreesInAngles)

fun<T:Number> MutableMatrix4<T, *, *>.rotate(axis: Vector3<T,*>, angle: AngleUnit) = rotate(axis.x, axis.y, axis.z, angle)

fun<T:Number> MutableMatrix4<T, *, *>.rotate(eulerX: AngleUnit, eulerY: AngleUnit, eulerZ: AngleUnit) {
    val temp = copyEmpty()
    temp.setFromEulerAngleUnits(eulerX, eulerY, eulerZ)
    this *= temp
    /*return synchronize {
        tmpMatA.setFromEulerAngleUnits(eulerX, eulerY, eulerZ)
        set(mul(tmpMatA, tmpMatB))
    }*/
}

/**
 * Inplace operation: Rotates this matrix by the given euler angles.
 */
fun<T:Number> MutableMatrix4<T, *, *>.rotateEulerOrder(eulerX: AngleUnit, eulerY: AngleUnit, eulerZ: AngleUnit, order: EulerOrder = EulerOrder.ZYX) {
    val a = eulerX.cos() + zero
    val b = eulerX.sin() + zero
    val c = eulerY.cos() + zero
    val d = eulerY.sin() + zero
    val e = eulerZ.cos() + zero
    val f = eulerZ.sin() + zero

    val t00: T
    val t01: T
    val t02: T
    val t10: T
    val t11: T
    val t12: T
    val t20: T
    val t21: T
    val t22: T

    when (order) {
        EulerOrder.XYZ -> {
            val ae = a * e; val af = a * f; val be = b * e; val bf = b * f

            t00 = c * e;        t01 = -c * f;       t02 = d
            t10 = af + be * d;  t11 = ae - bf * d;  t12 = - b * c
            t20 = bf - ae * d;  t21 = be + af * d;  t22 = a * c
        }
        EulerOrder.XZY -> {
            val ac = a * c; val ad = a * d; val bc = b * c; val bd = b * d

            t00 = c * e;        t01 = - f;          t02 = d * e
            t10 = ac * f + bd;  t11 = a * e;        t12 = ad * f - bc
            t20 = bc * f - ad;  t21 = b * e;        t22 = bd * f + ac
        }
        EulerOrder.YXZ -> {
            val ce = c * e; val cf = c * f; val de = d * e; val df = d * f

            t00 = ce + df * b;  t01 = de * b - cf;  t02 = a * d
            t10 = a * f;        t11 = a * e;        t12 = -b
            t20 = cf * b - de;  t21 = df + ce * b;  t22 = a * c
        }
        EulerOrder.YZX -> {
            val ac = a * c; val ad = a * d; val bc = b * c; val bd = b * d

            t00 = c * e;        t01 = bd - ac * f;  t02 = bc * f + ad
            t10 = f;            t11 = a * e;        t12 = - b * e
            t20 = - d * e;      t21 = ad * f + bc;  t22 = ac - bd * f
        }
        EulerOrder.ZXY -> {
            val ce = c * e; val cf = c * f; val de = d * e; val df = d * f

            t00 = ce - df * b;  t01 = - a * f;      t02 = de + cf * b
            t10 = cf + de * b;  t11 = a * e;        t12 = df - ce * b
            t20 = - a * d;      t21 = b;            t22 = a * c
        }
        EulerOrder.ZYX -> {
            val ae = a * e; val af = a * f; val be = b * e; val bf = b * f

            t00 = c * e;        t01 = be * d - af;  t02 = ae * d + bf
            t10 = c * f;        t11 = bf * d + ae;  t12 = af * d - be
            t20 = - d;          t21 = b * c;        t22 = a * c
        }
    }

     mul33(
        t00, t01, t02,
        t10, t11, t12,
        t20, t21, t22
    )
}


fun<T:Number> MutableMatrix4<T, *, *>.rotate(rotationMat: Matrix3<T, *, *>) {
    val temp = copyIdentity()
    temp.set(rotationMat)
    this *= temp
    /*return synchronize {
        tmpMatA.setToIdentity().set(rotationMat)
        set(mul(tmpMatA, tmpMatB))
    }*/
}

fun<T:Number> MutableMatrix4<T, *, *>.rotate(quaternion: Quaternion<T,*>) {
    val temp = copyIdentity()
    temp.setComponent(quaternion)
    this *= temp
    /*return synchronize {
        tmpMatA.setToIdentity().set(quaternion)
        set(mul(tmpMatA, tmpMatB))
    }*/
}

fun<T:Number> MutableMatrix4<T, *, *>.rotateX(angle: AngleUnit){
    val sin: Float
    val cos: Float
    if (angle == AngleUnit.PI || angle == -AngleUnit.PI) {
        cos = -1f
        sin = 0f
    } else if (angle == AngleUnit.PI * 0.5f || angle == -AngleUnit.PI * 1.5f) {
        cos = 0f
        sin = 1f
    } else if (angle == -AngleUnit.PI * 0.5f || angle == AngleUnit.PI * 1.5f) {
        cos = 0f
        sin = -1f
    } else {
        sin = angle.sinF()
        cos = angle.cosF()
    }
    val rm11 = cos
    val rm12 = sin
    val rm21 = -sin
    val rm22 = cos

    // add temporaries for dependent values
    val nm10 = this[1, 0] * rm11 + this[2, 0] * rm12
    val nm11 = this[1, 1] * rm11 + this[2, 1] * rm12
    val nm12 = this[1, 2] * rm11 + this[2, 2] * rm12
    val nm13 = this[1, 3] * rm11 + this[2, 3] * rm12
    // set non-dependent values directly
    this[2, 0] = this[1, 0] * rm21 + this[2, 0] * rm22
    this[2, 1] = this[1, 1] * rm21 + this[2, 1] * rm22
    this[2, 2] = this[1, 2] * rm21 + this[2, 2] * rm22
    this[2, 3] = this[1, 3] * rm21 + this[2, 3] * rm22
    // set other values
    this[1, 0] = nm10
    this[1, 1] = nm11
    this[1, 2] = nm12
    this[1, 3] = nm13

}

fun<T:Number> MutableMatrix4<T, *, *>.rotateY(angle: AngleUnit){
    val sin: Float
    val cos: Float
    if (angle == AngleUnit.PI || angle == -AngleUnit.PI) {
        cos = -1f
        sin = 0f
    } else if (angle == AngleUnit.PI * 0.5f || angle == -AngleUnit.PI * 1.5f) {
        cos = 0f
        sin = 1f
    } else if (angle == -AngleUnit.PI * 0.5f || angle == AngleUnit.PI * 1.5f) {
        cos = 0f
        sin = -1f
    } else {
        sin = angle.sinF()
        cos = angle.cosF()
    }
    val rm00 = cos
    val rm02 = -sin
    val rm20 = sin
    val rm22 = cos

    // add temporaries for dependent values
    val nm00 = this[0, 0] * rm00 + this[2, 0] * rm02
    val nm01 = this[0, 1] * rm00 + this[2, 1] * rm02
    val nm02 = this[0, 2] * rm00 + this[2, 2] * rm02
    val nm03 = this[0, 3] * rm00 + this[2, 3] * rm02
    // set non-dependent values directly
    this[2, 0] = this[0, 0] * rm20 + this[2, 0] * rm22
    this[2, 1] = this[0, 1] * rm20 + this[2, 1] * rm22
    this[2, 2] = this[0, 2] * rm20 + this[2, 2] * rm22
    this[2, 3] = this[0, 3] * rm20 + this[2, 3] * rm22
    // set other values
    this[0, 0] = nm00
    this[0, 1] = nm01
    this[0, 2] = nm02
    this[0, 3] = nm03
    
}

fun<T:Number> MutableMatrix4<T, *, *>.rotateZ( angle: AngleUnit) {
    val sin: Float
    val cos: Float
    if (angle == AngleUnit.PI || angle == -AngleUnit.PI) {
        cos = -1f
        sin = 0f
    } else if (angle == AngleUnit.PI * 0.5f || angle == -AngleUnit.PI * 1.5f) {
        cos = 0f
        sin = 1f
    } else if (angle == -AngleUnit.PI * 0.5f || angle == AngleUnit.PI * 1.5f) {
        cos = 0f
        sin = -1f
    } else {
        sin = angle.sinF()
        cos = angle.cosF()
    }
    val rm00 = cos
    val rm01 = sin
    val rm10 = -sin
    val rm11 = cos
    // add temporaries for dependent values
    val nm00 = this[0, 0] * rm00 + this[1, 0] * rm01
    val nm01 = this[0, 1] * rm00 + this[1, 1] * rm01
    val nm02 = this[0, 2] * rm00 + this[1, 2] * rm01
    val nm03 = this[0, 3] * rm00 + this[1, 3] * rm01
    // set non-dependent values directly
    this[1, 0] = this[0, 0] * rm10 + this[1, 0] * rm11
    this[1, 1] = this[0, 1] * rm10 + this[1, 1] * rm11
    this[1, 2] = this[0, 2] * rm10 + this[1, 2] * rm11
    this[1, 3] = this[0, 3] * rm10 + this[1, 3] * rm11
    // set other values
    this[0, 0] = nm00
    this[0, 1] = nm01
    this[0, 2] = nm02
    this[0, 3] = nm03
    
}

/**
 * Returns the euler angle representation of this matrix's rotation component. Angles are returned in degrees
 * in the given [eulersDeg] vector. Euler angle order can bes set via [order], default is ZYX.
 */
fun<T:Number> Matrix4<T, *, *>.getEulerAngles(eulersDeg: MutableVector3<T,*>, order: EulerOrder = EulerOrder.ZYX){
    var sx = sqrt(m00*m00 + m10*m10 + m20*m20)
    val sy = sqrt(m01*m01 + m11*m11 + m21*m21)
    val sz = sqrt(m02*m02 + m12*m12 + m22*m22)
    if (det < 0.0) {
        sx *= -1.0
    }

    val r00 = m00 / sx; val r01 = m01 / sy; val r02 = m02 / sz
    val r10 = m10 / sx; val r11 = m11 / sy; val r12 = m12 / sz
    val r20 = m20 / sx; val r21 = m21 / sy; val r22 = m22 / sz

    when (order) {
        EulerOrder.XYZ -> {
            eulersDeg.y = asin(clamp(r02,-one, one).toDouble()).toDouble().radiansToDegrees() +zero
            if (abs(r02) < 0.9999998807907104) {
                eulersDeg.x = atan2(-r12, r22).toDouble().radiansToDegrees() +zero
                eulersDeg.z = atan2(-r01, r00).toDouble().radiansToDegrees() +zero
            } else {
                eulersDeg.x = atan2(r21, r11).toDouble().radiansToDegrees() +zero
                eulersDeg.z = zero
            }
        }
        EulerOrder.XZY -> {
            eulersDeg.z = asin(clamp(-r01,-one, one).toDouble()).toDouble().radiansToDegrees() +zero
            if (abs(r01) < 0.9999998807907104) {
                eulersDeg.x = atan2(r21, r11).toDouble().radiansToDegrees() +zero
                eulersDeg.y = atan2(r02, r00).toDouble().radiansToDegrees() +zero
            } else {
                eulersDeg.x = atan2(-r12, r22).toDouble().radiansToDegrees() +zero
                eulersDeg.y = zero
            }
        }
        EulerOrder.YXZ -> {
            eulersDeg.x = asin(clamp(-r12,-one, one).toDouble()).radiansToDegrees() +zero
            if (abs(r12) < 0.9999998807907104) {
                eulersDeg.y = atan2(r02, r22).toDouble().radiansToDegrees() +zero
                eulersDeg.z = atan2(r10, r11).toDouble().radiansToDegrees() +zero
            } else {
                eulersDeg.y = atan2(-r20, r00).toDouble().radiansToDegrees() +zero
                eulersDeg.z = zero
            }
        }
        EulerOrder.YZX -> {
            eulersDeg.z = asin(clamp(r10,-one, one).toDouble()).radiansToDegrees() +zero
            if (abs(r10) < 0.9999998807907104) {
                eulersDeg.x = atan2(-r12, r11).toDouble().radiansToDegrees() +zero
                eulersDeg.y = atan2(-r20, r00).toDouble().radiansToDegrees() +zero
            } else {
                eulersDeg.x = zero
                eulersDeg.y = atan2(r02, r22).toDouble().radiansToDegrees() +zero
            }
        }
        EulerOrder.ZXY -> {
            eulersDeg.x = asin(clamp(r21,-one, one).toDouble()).radiansToDegrees() +zero
            if (abs(r21) < 0.9999998807907104) {
                eulersDeg.y = atan2(-r20, r22).toDouble().radiansToDegrees() +zero
                eulersDeg.z = atan2(-r01, r11).toDouble().radiansToDegrees() +zero
            } else {
                eulersDeg.y = zero
                eulersDeg.z = atan2(r10, r00).toDouble().radiansToDegrees() +zero
            }
        }
        EulerOrder.ZYX -> {
            eulersDeg.y = asin(clamp(-r20,-one, one).toDouble()).radiansToDegrees() +zero
            if (abs(r20) < 0.9999998807907104) {
                eulersDeg.x = atan2(r21, r22).toDouble().radiansToDegrees() +zero
                eulersDeg.z = atan2(r10, r00).toDouble().radiansToDegrees() +zero
            } else {
                eulersDeg.x = zero
                eulersDeg.z = atan2(-r01, r11).toDouble().radiansToDegrees() +zero
            }
        }
    }
}

